
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sim_plane.c
  * @author     baiyang
  * @date       2022-12-9
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sim_plane.h"

#include <string.h>

#include <rtthread.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
void sim_plane_update(sim_aircraft_t aircraft, const struct sitl_input *input);

static float liftCoeff(sim_plane_const_t plane, float alpha);
static float dragCoeff(sim_plane_const_t plane, float alpha);
static Vector3f_t getTorque(sim_plane_const_t plane, float inputAileron, float inputElevator, float inputRudder, float inputThrust, const Vector3f_t *force);
static Vector3f_t getForce(sim_plane_const_t plane, float inputAileron, float inputElevator, float inputRudder);
/*----------------------------------variable----------------------------------*/
static struct plane_coefficient coefficient_default = PLANE_COEFFICIENT_DEFAULT;

struct sim_aircraft_ops plane_ops = {.set_start_location = NULL,
                                     .set_interface_ports = NULL,
                                     .update = sim_plane_update,
                                     .set_config = NULL,
                                     .rangefinder_beam_width = NULL,
                                     .perpendicular_distance_to_rangefinder_surface = NULL,
                                     .on_ground = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sim_plane_ctor(sim_plane_t plane, sim_aircraft_ops_t ops, const char *frame_str)
{
    memset(plane, 0, sizeof(struct sim_plane));

    if (!ops) {
        sim_aircraft_ctor(&plane->aircraft, &plane_ops, frame_str);
    } else {
        sim_aircraft_ctor(&plane->aircraft, ops, frame_str);
    }

    plane->hover_throttle = 0.7f;
    plane->air_density = 1.225f;

    plane->coefficient = coefficient_default;

    plane->throttle_servo = 2;
    plane->choke_servo    = 14;
    plane->ignition_servo = 12;
    plane->starter_servo  = 13;
    plane->slewrate       = 100;

    sim_icengine_ctor(&plane->icengine, plane->throttle_servo, plane->choke_servo, plane->ignition_servo, plane->starter_servo, plane->slewrate, true);

    sim_aircraft_t aircraft = &plane->aircraft;

    aircraft->mass = 2.0f;

    /*
       scaling from motor power to Newtons. Allows the plane to hold
       vertically against gravity when the motor is at hover_throttle
    */
    plane->thrust_scale = (aircraft->mass * GRAVITY_MSS) / plane->hover_throttle;
    aircraft->frame_height = 0.1f;

    aircraft->ground_behavior = GROUND_BEHAVIOR_FWD_ONLY;
    aircraft->lock_step_scheduled = true;

    if (strstr(frame_str, "-heavy")) {
        aircraft->mass = 8;
    }
    if (strstr(frame_str, "-jet")) {
        // a 22kg "jet", level top speed is 102m/s
        aircraft->mass = 22;
        plane->thrust_scale = (aircraft->mass * GRAVITY_MSS) / plane->hover_throttle;
    }
    if (strstr(frame_str, "-revthrust")) {
        plane->reverse_thrust = true;
    }
    if (strstr(frame_str, "-elevon")) {
        plane->elevons = true;
    } else if (strstr(frame_str, "-vtail")) {
        plane->vtail = true;
    } else if (strstr(frame_str, "-dspoilers")) {
        plane->dspoilers = true;
    }
    if (strstr(frame_str, "-elevrev")) {
        plane->reverse_elevator_rudder = true;
    }
    if (strstr(frame_str, "-catapult")) {
        plane->have_launcher = true;
        plane->launch_accel = 15;
        plane->launch_time = 2;
    }
    if (strstr(frame_str, "-bungee")) {
        plane->have_launcher = true;
        plane->launch_accel = 7;
        plane->launch_time = 4;
    }
    if (strstr(frame_str, "-throw")) {
        plane->have_launcher = true;
        plane->launch_accel = 25;
        plane->launch_time = 0.4f;
    }
    if (strstr(frame_str, "-tailsitter")) {
        plane->tailsitter = true;
        aircraft->ground_behavior = GROUND_BEHAVIOR_TAILSITTER;
        plane->thrust_scale *= 1.5;
    }

    if (strstr(frame_str, "-3d")) {
        plane->aerobatic = true;
        plane->thrust_scale *= 1.5;
        // setup parameters for plane-3d
        //AP_Param::load_defaults_file("@ROMFS/models/plane.parm", false);
        //AP_Param::load_defaults_file("@ROMFS/models/plane-3d.parm", false);
    }
    
    if (strstr(frame_str, "-ice")) {
        plane->ice_engine = true;
    }

    if (strstr(frame_str, "-soaring")) {
        aircraft->mass = 2.0;
        plane->coefficient.c_drag_p = 0.05f;
    }
}

/* static object creator */
sim_aircraft_t sim_plane_create(const char *frame_str)
{
    sim_plane_t plane = (sim_plane_t)rt_malloc(sizeof(struct sim_plane));

    if (!plane) {
        return NULL;
    }

    sim_plane_ctor(plane, NULL, frame_str);

    return (sim_aircraft_t)plane;
}

/*
  update the plane simulation by one time step
 */
void sim_plane_update(sim_aircraft_t aircraft, const struct sitl_input *input)
{
    sim_plane_t plane = (sim_plane_t)aircraft;

    Vector3f_t rot_accel = VECTOR3F_DEFAULT_VALUE;

    sim_aircraft_update_wind(aircraft, input);

    sim_plane_calculate_forces(plane, input, &rot_accel);

    sim_aircraft_update_dynamics(aircraft, &rot_accel);
    sim_aircraft_update_external_payload(aircraft, input);

    // update lat/lon/altitude
    sim_aircraft_update_position(aircraft);
    sim_aircraft_time_advance(aircraft);

    // update magnetic field
    sim_aircraft_update_mag_field_bf(aircraft);
}

void sim_plane_calculate_forces(sim_plane_t plane, const struct sitl_input *input, Vector3f_t *rot_accel)
{
    sim_aircraft_t aircraft = &plane->aircraft;

    float aileron  = sim_aircraft_filtered_servo_angle(aircraft, input, 0);
    float elevator = sim_aircraft_filtered_servo_angle(aircraft, input, 1);
    float rudder   = sim_aircraft_filtered_servo_angle(aircraft, input, 3);
    bool launch_triggered = input->servos[6] > 1700;
    float throttle;
    if (plane->reverse_elevator_rudder) {
        elevator = -elevator;
        rudder = -rudder;
    }
    if (plane->elevons) {
        // fake an elevon plane
        float ch1 = aileron;
        float ch2 = elevator;
        aileron  = (ch2-ch1)/2.0f;
        // the minus does away with the need for RC2_REVERSED=-1
        elevator = -(ch2+ch1)/2.0f;

        // assume no rudder
        rudder = 0;
    } else if (plane->vtail) {
        // fake a vtail plane
        float ch1 = elevator;
        float ch2 = rudder;
        // this matches VTAIL_OUTPUT==2
        elevator = (ch2-ch1)/2.0f;
        rudder   = (ch2+ch1)/2.0f;
    } else if (plane->dspoilers) {
        // fake a differential spoiler plane. Use outputs 1, 2, 4 and 5
        float dspoiler1_left = sim_aircraft_filtered_servo_angle(aircraft, input, 0);
        float dspoiler1_right = sim_aircraft_filtered_servo_angle(aircraft, input, 1);
        float dspoiler2_left = sim_aircraft_filtered_servo_angle(aircraft, input, 3);
        float dspoiler2_right = sim_aircraft_filtered_servo_angle(aircraft, input, 4);
        float elevon_left  = (dspoiler1_left + dspoiler2_left)/2;
        float elevon_right = (dspoiler1_right + dspoiler2_right)/2;
        aileron  = (elevon_right-elevon_left)/2;
        elevator = (elevon_left+elevon_right)/2;
        rudder = fabsf(dspoiler1_right - dspoiler2_right)/2 - fabsf(dspoiler1_left - dspoiler2_left)/2;
    }
    //printf("Aileron: %.1f elevator: %.1f rudder: %.1f\n", aileron, elevator, rudder);

    if (plane->reverse_thrust) {
        throttle = sim_aircraft_filtered_servo_angle(aircraft, input, 2);
    } else {
        throttle = sim_aircraft_filtered_servo_range(aircraft, input, 2);
    }
    
    float thrust     = throttle;

    aircraft->battery_voltage = aircraft->sitl->batt_voltage - 0.7f*throttle;
    aircraft->battery_current = (aircraft->battery_voltage/aircraft->sitl->batt_voltage)*50.0f*sq(throttle);

    if (plane->ice_engine) {
        thrust = sim_icengine_update(&plane->icengine, input);
    }

    // calculate angle of attack
    plane->angle_of_attack = atan2f(aircraft->velocity_air_bf.z, aircraft->velocity_air_bf.x);
    plane->beta = atan2f(aircraft->velocity_air_bf.y,aircraft->velocity_air_bf.x);

    if (plane->tailsitter || plane->aerobatic) {
        /*
          tailsitters get 4x the control surfaces
         */
        aileron *= 4;
        elevator *= 4;
        rudder *= 4;
    }
    
    Vector3f_t force = getForce(plane, aileron, elevator, rudder);
    *rot_accel = getTorque(plane, aileron, elevator, rudder, thrust, &force);

    if (plane->have_launcher) {
        /*
          simple simulation of a launcher
         */
        if (launch_triggered) {
            uint64_t now = time_millis64();
            if (plane->launch_start_ms == 0) {
                plane->launch_start_ms = now;
            }
            if (now - plane->launch_start_ms < plane->launch_time*1000) {
                force.x += aircraft->mass * plane->launch_accel;
                force.z += aircraft->mass * plane->launch_accel/3;
            }
        } else {
            // allow reset of catapult
            plane->launch_start_ms = 0;
        }
    }
    
    // simulate engine RPM
    aircraft->motor_mask |= (1U<<2);
    aircraft->rpm[2] = thrust * 7000;

    // scale thrust to newtons
    thrust *= plane->thrust_scale;

    aircraft->accel_body.x = thrust + force.x;
    aircraft->accel_body.y = force.y;
    aircraft->accel_body.z = force.z;

    aircraft->accel_body.x /= aircraft->mass;
    aircraft->accel_body.y /= aircraft->mass;
    aircraft->accel_body.z /= aircraft->mass;

    // add some noise
    if (plane->thrust_scale > 0) {
        sim_aircraft_add_noise(aircraft, fabsf(thrust) / plane->thrust_scale);
    }

    if (sim_aircraft_on_ground(aircraft) && !plane->tailsitter) {
        // add some ground friction
        Vector3f_t vel_body;
        matrix3f_mul_transpose_vec(&vel_body, &aircraft->dcm, &aircraft->velocity_ef);

        aircraft->accel_body.x -= vel_body.x * 0.3f;
    }
}

/*
  the following functions are from last_letter
  https://github.com/Georacer/last_letter/blob/master/last_letter/src/aerodynamicsLib.cpp
  many thanks to Georacer!
 */
static float liftCoeff(sim_plane_const_t plane, float alpha)
{
    const float alpha0 = plane->coefficient.alpha_stall;
    const float M = plane->coefficient.mcoeff;
    const float c_lift_0 = plane->coefficient.c_lift_0;
    const float c_lift_a0 = plane->coefficient.c_lift_a;

    // clamp the value of alpha to avoid exp(90) in calculation of sigmoid
    const float max_alpha_delta = 0.8f;
    if (alpha-alpha0 > max_alpha_delta) {
        alpha = alpha0 + max_alpha_delta;
    } else if (alpha0-alpha > max_alpha_delta) {
        alpha = alpha0 - max_alpha_delta;
    }
    double sigmoid = ( 1+exp(-M*(alpha-alpha0))+exp(M*(alpha+alpha0)) ) / (1+exp(-M*(alpha-alpha0))) / (1+exp(M*(alpha+alpha0)));
    double linear = (1.0-sigmoid) * (c_lift_0 + c_lift_a0*alpha); //Lift at small AoA
    double flatPlate = sigmoid*(2*copysign(1,alpha)*pow(sin(alpha),2)*cos(alpha)); //Lift beyond stall

    float result  = (float)(linear+flatPlate);
    return result;
}

static float dragCoeff(sim_plane_const_t plane, float alpha)
{
    const float b = plane->coefficient.b;
    const float s = plane->coefficient.s;
    const float c_drag_p = plane->coefficient.c_drag_p;
    const float c_lift_0 = plane->coefficient.c_lift_0;
    const float c_lift_a0 = plane->coefficient.c_lift_a;
    const float oswald = plane->coefficient.oswald;

    double AR = pow(b,2)/s;
    double c_drag_a = c_drag_p + pow(c_lift_0+c_lift_a0*alpha,2)/(M_PI*oswald*AR);

    return (float)c_drag_a;
}

// Torque calculation function
static Vector3f_t getTorque(sim_plane_const_t plane, float inputAileron, float inputElevator, float inputRudder, float inputThrust, const Vector3f_t *force)
{
    sim_aircraft_const_t aircraft = &plane->aircraft;

    float alpha = plane->angle_of_attack;

    //calculate aerodynamic torque
    float effective_airspeed = aircraft->airspeed;

    if (plane->tailsitter || plane->aerobatic) {
        /*
          tailsitters get airspeed from prop-wash
         */
        effective_airspeed += inputThrust * 20;

        // reduce effective angle of attack as thrust increases
        alpha *= math_constrain_float(1 - inputThrust, 0, 1);
    }
    
    const float s = plane->coefficient.s;
    const float c = plane->coefficient.c;
    const float b = plane->coefficient.b;
    const float c_l_0 = plane->coefficient.c_l_0;
    const float c_l_b = plane->coefficient.c_l_b;
    const float c_l_p = plane->coefficient.c_l_p;
    const float c_l_r = plane->coefficient.c_l_r;
    const float c_l_deltaa = plane->coefficient.c_l_deltaa;
    const float c_l_deltar = plane->coefficient.c_l_deltar;
    const float c_m_0 = plane->coefficient.c_m_0;
    const float c_m_a = plane->coefficient.c_m_a;
    const float c_m_q = plane->coefficient.c_m_q;
    const float c_m_deltae = plane->coefficient.c_m_deltae;
    const float c_n_0 = plane->coefficient.c_n_0;
    const float c_n_b = plane->coefficient.c_n_b;
    const float c_n_p = plane->coefficient.c_n_p;
    const float c_n_r = plane->coefficient.c_n_r;
    const float c_n_deltaa = plane->coefficient.c_n_deltaa;
    const float c_n_deltar = plane->coefficient.c_n_deltar;
    const Vector3f_t *CGOffset = &plane->coefficient.CGOffset;
    
    float rho = plane->air_density;

    //read angular rates
    double p = aircraft->gyro.x;
    double q = aircraft->gyro.y;
    double r = aircraft->gyro.z;

    double qbar = 1.0/2.0*rho*pow(effective_airspeed,2)*s; //Calculate dynamic pressure
    double la, na, ma;
    if (math_flt_zero(effective_airspeed))
    {
        la = 0;
        ma = 0;
        na = 0;
    }
    else
    {
        la = qbar*b*(c_l_0 + c_l_b*plane->beta + c_l_p*b*p/(2*effective_airspeed) + c_l_r*b*r/(2*effective_airspeed) + c_l_deltaa*inputAileron + c_l_deltar*inputRudder);
        ma = qbar*c*(c_m_0 + c_m_a*alpha + c_m_q*c*q/(2*effective_airspeed) + c_m_deltae*inputElevator);
        na = qbar*b*(c_n_0 + c_n_b*plane->beta + c_n_p*b*p/(2*effective_airspeed) + c_n_r*b*r/(2*effective_airspeed) + c_n_deltaa*inputAileron + c_n_deltar*inputRudder);
    }


    // Add torque to force misalignment with CG
    // r x F, where r is the distance from CoG to CoL
    la +=  CGOffset->y * force->z - CGOffset->z * force->y;
    ma += -CGOffset->x * force->z + CGOffset->z * force->x;
    na += -CGOffset->y * force->x + CGOffset->x * force->y;

    Vector3f_t torque= {(float)la, (float)ma, (float)na};

    return torque;
}

// Force calculation function from last_letter
static Vector3f_t getForce(sim_plane_const_t plane, float inputAileron, float inputElevator, float inputRudder)
{
    sim_aircraft_const_t aircraft = &plane->aircraft;

    const float alpha = plane->angle_of_attack;
    const float c_drag_q = plane->coefficient.c_drag_q;
    const float c_lift_q = plane->coefficient.c_lift_q;
    const float s = plane->coefficient.s;
    const float c = plane->coefficient.c;
    const float b = plane->coefficient.b;
    const float c_drag_deltae = plane->coefficient.c_drag_deltae;
    const float c_lift_deltae = plane->coefficient.c_lift_deltae;
    const float c_y_0 = plane->coefficient.c_y_0;
    const float c_y_b = plane->coefficient.c_y_b;
    const float c_y_p = plane->coefficient.c_y_p;
    const float c_y_r = plane->coefficient.c_y_r;
    const float c_y_deltaa = plane->coefficient.c_y_deltaa;
    const float c_y_deltar = plane->coefficient.c_y_deltar;
    
    float rho = plane->air_density;

    //request lift and drag alpha-coefficients from the corresponding functions
    double c_lift_a = liftCoeff(plane, alpha);
    double c_drag_a = dragCoeff(plane, alpha);

    //convert coefficients to the body frame
    double c_x_a = -c_drag_a*cos(alpha)+c_lift_a*sin(alpha);
    double c_x_q = -c_drag_q*cos(alpha)+c_lift_q*sin(alpha);
    double c_z_a = -c_drag_a*sin(alpha)-c_lift_a*cos(alpha);
    double c_z_q = -c_drag_q*sin(alpha)-c_lift_q*cos(alpha);

    //read angular rates
    double p = aircraft->gyro.x;
    double q = aircraft->gyro.y;
    double r = aircraft->gyro.z;

    //calculate aerodynamic force
    double qbar = 1.0/2.0*rho*pow(aircraft->airspeed,2)*s; //Calculate dynamic pressure
    double ax, ay, az;
    if (math_flt_zero(aircraft->airspeed))
    {
        ax = 0;
        ay = 0;
        az = 0;
    }
    else
    {
        ax = qbar*(c_x_a + c_x_q*c*q/(2*aircraft->airspeed) - c_drag_deltae*cos(alpha)*fabs(inputElevator) + c_lift_deltae*sin(alpha)*inputElevator);
        // split c_x_deltae to include "abs" term
        ay = qbar*(c_y_0 + c_y_b*plane->beta + c_y_p*b*p/(2*aircraft->airspeed) + c_y_r*b*r/(2*aircraft->airspeed) + c_y_deltaa*inputAileron + c_y_deltar*inputRudder);
        az = qbar*(c_z_a + c_z_q*c*q/(2*aircraft->airspeed) - c_drag_deltae*sin(alpha)*fabs(inputElevator) - c_lift_deltae*cos(alpha)*inputElevator);
        // split c_z_deltae to include "abs" term
    }

    Vector3f_t force= {(float)ax, (float)ay, (float)az};

    return force;
}
/*------------------------------------test------------------------------------*/


